#include "config.hpp"
#include <iostream>

using namespace std;
using namespace YAML;

bool Config::LoadFile(std::string path)
{
    Node file = YAML::LoadFile(path);
    if (!file)
    {
        cout << "Load yaml file error" << endl;
        return false;
    }

    LoadSerialParams(IMU, file["IMU"]);
    LoadSerialParams(MototrBus, file["Bus"]);
    LoadSerialParams(SerialStick, file["SerialStick"]);
    LoadMotorParams(leftMotor, file["Bus"]["LeftMotor"]);
    LoadMotorParams(rightMotor, file["Bus"]["RightMotor"]);

    Node jsNode = file["Joystick"];
    joystick.device = jsNode["Dev"].as<string>();

    Node socketNode = file["Socket"];
    socket.ip = socketNode["IP"].as<string>();
    socket.port = socketNode["Port"].as<int>();

    LoadPIDParams(gyroPID, file["GyroPID"]);
    LoadPIDParams(anglePID, file["AnglePID"]);
    LoadPIDParams(speedPID, file["SpeedPID"]);
    LoadPIDParams(rotatePID, file["RotatePID"]);

    maxAngle = file["MaxAngle"].as<float>();
    maxOutput = file["MaxOutput"].as<float>();
    errorAngle = file["ErrorAngle"].as<float>();
    errorRPM = file["ErrorRPM"].as<float>();
    zeroPoint = file["ZeroPoint"].as<float>();

    LoadAixParams(moveAix, file["MoveAix"]);
    LoadAixParams(rotateAix, file["RotateAix"]);

    return true;
}

void Config::LoadPIDParams(PIDParams &param, const YAML::Node &node)
{
    param.P = node["P"].as<float>();
    param.I = node["I"].as<float>();
    param.D = node["D"].as<float>();
}

void Config::LoadMotorParams(MotorCfg &cfg, const YAML::Node &node)
{
    cfg.address = node["Address"].as<int>();
    cfg.maxERPM = node["MaxERPM"].as<int>();
    cfg.pole = node["Pole"].as<int>();
}

void Config::LoadSerialParams(SerialCfg &cfg, const YAML::Node &node)
{
    cfg.device = node["Dev"].as<string>();
    cfg.baudrate = node["BaudRate"].as<int>();
    cfg.type = node["Type"].as<string>();
}

void Config::LoadAixParams(AixCfg &cfg, const YAML::Node &node)
{
    cfg.number = node["AixNumber"].as<int>();
    cfg.number = node["Invert"].as<bool>();
    cfg.range = node["Range"].as<float>();
}
